#include <iostream>
#include "myslam/common_include.h"
#include "myslam/config.h"
#include "myslam/camera.h"
#include "myslam/visual_odometry.h"
#include "myslam/save_pose.h"
#include <fstream>
#include <boost/timer.hpp>
#include<opencv2/opencv.hpp>
#include <opencv2/viz.hpp>



using namespace std;

int main()
{
    cout <<"staring ......."<<endl;
    ofstream fout( "../bin/pose.txt" );

    myslam::Config::setParameterFile("../config/default.yaml");
    string dataset_dir = myslam::Config::get<string>("dataset_dir");
    myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry);
    cout<<dataset_dir<<endl;
    ifstream fin(dataset_dir + "/associate.txt");
    if(!fin)
    {
        cout<<"associate.txt is not found!!!"<<endl;
        return  -1;
    }


    vector<string> rgb_files, depth_files;
    vector<double> rgb_times,depth_times;
    while(!fin.eof())
    {

        string rgb_time,rgb_file,depth_time,depth_file;
        fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
        rgb_times.push_back(atof(rgb_time.c_str()));
        depth_times.push_back(atof(depth_time.c_str()));
        rgb_files.push_back(dataset_dir+"/" + rgb_file);
        depth_files.push_back(dataset_dir +"/"+ depth_file);
        if(fin.good()== false)
            break;
    }

    myslam::Camera::Ptr camera(new myslam::Camera);
    cv::viz::Viz3d vis("Visual Odometry");
    vis.setWindowPosition(Point(800,100));
    cv::viz::WCoordinateSystem world_coor(1.0),camera_coor(0.5),camera_coor_BA(0.5);
    cv::Point3d cam_pos(0,-1.0,-1.0),cam_focal_point(0,0,0),cam_y_dir(0,1,0);
    cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos,cam_focal_point,cam_y_dir);
    vis.setViewerPose(cam_pose);

    world_coor.setRenderingProperty(cv::viz::LINE_WIDTH,2.0);
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH,1.0);
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH,2.0);
    vis.showWidget("World",world_coor);
    vis.showWidget("Camera",camera_coor);
    vis.showWidget("Camera1",camera_coor_BA);

    cout<<"real total "<< rgb_files.size() <<" entries"<<endl;

    Point3d point_begin(0.0, 0.0, 0.0);
    Point3d point_begin_BA(0.0, 0.0, 0.0);
    Point3d point_end;
    Point3d point_end_BA;

    myslam::SavePose::Ptr save_pose(new myslam::SavePose);

    for(size_t i =0;i<rgb_files.size();i++)
    {
        cout <<"the "<<i<< "frame"<<endl;
        cv::Mat color = cv::imread(rgb_files[i]);
        cv::Mat depth = cv::imread(depth_files[i]);

        if(color.data == nullptr || depth.data ==nullptr)
        {
            break;
        }

        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();

        pFrame->camera_ = camera;
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];
        boost::timer timer;

        vo->addFrame(pFrame);
        cout<<"timmer of i = "<<i<<endl;

        cout<<"VO costs time : "<<timer.elapsed()<<endl;

        if(vo->state_ == myslam::VisualOdometry::LOST)
            break;
        // Pw*T_c_w_ 为当前帧下的Pc，求Pw的下位姿Twc为Tcw的逆
        SE3d Twc = pFrame->T_c_w_.inverse();
        SE3d Twc_BA = pFrame->T_c_w_BA_.inverse();
        cout<<"---------------------"<<endl;


       // save_pose->save_pose(Twc_BA,rgb_times[i]);

        Eigen::Quaterniond q = Eigen::Quaterniond(Twc_BA.rotationMatrix());
        fout << to_string(rgb_times[i])<<" "
             << Twc_BA.translation()(0,0)<<" "
             << Twc_BA.translation()(1,0)<<" "
             << Twc_BA.translation()(2,0)<<" "
             << q.coeffs().transpose()(0)<<" "
             << q.coeffs().transpose()(1)<<" "
             << q.coeffs().transpose()(2)<<" "
             << q.coeffs().transpose()(3)<<endl;


        cout<<"SE3d Tcw in pFrame of "<<i<< "frame"<<endl;

        cv::Affine3d M(
                    cv::Affine3d::Mat3(
                        Twc.rotationMatrix()(0,0),Twc.rotationMatrix()(0,1),Twc.rotationMatrix()(0,2),
                        Twc.rotationMatrix()(1,0), Twc.rotationMatrix()(1,1), Twc.rotationMatrix()(1,2),
                        Twc.rotationMatrix()(2,0), Twc.rotationMatrix()(2,1), Twc.rotationMatrix()(2,2)
                        ),
                    cv::Affine3d::Vec3(
                        Twc.translation()(0,0),Twc.translation()(1,0),Twc.translation()(2,0)
                        )
                    );
        // 终点
        point_end = Point3d( Twc.translation()(0, 0),Twc.translation()(1, 0),Twc.translation()(2, 0));
        point_end_BA = Point3d(Twc_BA.translation()(0, 0),Twc_BA.translation()(1, 0),Twc_BA.translation()(2, 0));

        viz::WLine line(point_begin, point_end, cv::viz::Color::red());
        line.setRenderingProperty(cv::viz::LINE_WIDTH,3.0);
        string id = to_string(i);
        vis.showWidget(id, line);
        // 更新终点
        point_begin = point_end;

        cv::Affine3d M1(
                    cv::Affine3d::Mat3(
                        Twc_BA.rotationMatrix()(0,0),Twc_BA.rotationMatrix()(0,1),Twc_BA.rotationMatrix()(0,2),
                        Twc_BA.rotationMatrix()(1,0), Twc_BA.rotationMatrix()(1,1), Twc_BA.rotationMatrix()(1,2),
                        Twc_BA.rotationMatrix()(2,0), Twc_BA.rotationMatrix()(2,1), Twc_BA.rotationMatrix()(2,2)
                        ),
                    cv::Affine3d::Vec3(
                        Twc_BA.translation()(0,0),Twc_BA.translation()(1,0),Twc_BA.translation()(2,0)
                        )
                    );
        viz::WLine line_BA(point_begin_BA, point_end_BA, cv::viz::Color::green());
        line_BA.setRenderingProperty(cv::viz::LINE_WIDTH,2.0);
        vis.showWidget(to_string(i+1000), line_BA);
        point_begin_BA = point_end_BA;
        cv::imshow("image",color);
        cv::waitKey(1);

        vis.setWidgetPose("Camera",M);
        vis.setWidgetPose("Camera1",M1);
        vis.spinOnce(1,false);


    }



    //关闭文件输出流
    fout.close();
    return 0;
}
